﻿/*
 * Copyright © 2020, Wuxi Safover Intelligent Technology Co., Ltd. All rights reserved.
 * 
 * 无锡安永智能科技有限公司版权所有，未经许可，不得用于商业用途
 * 
 * Visit: https://www.safover.com/ 
 */

using log4net;
using System;
using System.IO.Ports;
using System.Reflection;
using System.Threading;
using Safover.LidarCollector.Core;
using Safover.LidarCollector.Models;

namespace Ec.Bewis
{
    // https://wenku.baidu.com/view/fbc53fa558f5f61fb7366697.html
    public class BewisElectronicCompass : ElectronicCompass
    {
        private static readonly ILog Log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

        #region Singleton
        public static ElectronicCompass Instance { get; } = new BewisElectronicCompass();

        private BewisElectronicCompass() { }
        #endregion

        public override string DeviceType => "BewisElectronicCompass";

        protected override void OnDataReceived(object sender, SerialDataReceivedEventArgs e)
        {

            byte[] packet = new byte[14];
            _ = Read(packet, 0, packet.Length);

            if (packet[3] != 0x84)
            {
                return;
            }

            PitchAngle = ToFloat(packet, 4);
            RollAngle = ToFloat(packet, 7);
            HeadingAngle = ToFloat(packet, 10);

            DeviceReadingsUpdated?.Invoke(this, new ElectronicCompassReadingsUpdatedEventArgs 
            {
                PitchAngle = (float)PitchAngle,
                RollAngle = (float)RollAngle,
                HeadingAngle = (float)HeadingAngle,
            });
        }

        private float? ToFloat(byte[] buffer, int offset)
        {
            try
            {
                var str = $"{buffer[offset]:x2}{buffer[offset + 1]:x2}{buffer[offset + 2]:x2}";

                int positive = str.StartsWith("0") ? 1 : -1;

                var valueStr = $"{str.Substring(1, 3)}.{str.Substring(4, 2)}";

                var value = float.Parse(valueStr) * positive;

                return value;
            }
            catch (Exception e)
            {
                Log.Error($"{DeviceType}: {e.Message}.", e);
            }

            return null;
        }

        public override ElectronicCompassAngles? QueryAngles()
        {
            HeadingAngle = null;
            PitchAngle = null;
            RollAngle = null;

            byte[] cmd = new byte[] { 0x77, 0x04, 0x00, 0x04, 0x08 };

            if (Write(cmd))
            {
                Thread.Sleep(100);
            }

            return PitchAngle != null
                ? (ElectronicCompassAngles?)new ElectronicCompassAngles
                {
                    RollAngle = (float)RollAngle,
                    HeadingAngle = (float)HeadingAngle,
                    PitchAngle = (float)PitchAngle,
                }
                : null;
        }
    }
}
